#include "VisionApi.h"


#include <QDebug>
#include <random>
#include <QFile>
#include <QDir>


VISION_EXPORT bool VisionApi::isFileExist(const char* filePath)
{
	QFile file(filePath);
	return file.exists();
}

VISION_EXPORT bool VisionApi::isDirExist(const char* dir)
{
	QDir destDir(dir);
	return destDir.exists();
}

VISION_EXPORT bool VisionApi::fitCircle_LeastSquares(const std::vector<cv::Point2d>& points, cv::Point2d* center, double* radius, double* rmse)
{
	int n = points.size();
	if (n < 3)
	{
		return false;
	}

	// 计算所需的统计量
	float sum_x = 0, sum_y = 0, sum_x2 = 0, sum_y2 = 0, sum_x3 = 0, sum_y3 = 0, sum_xy = 0, sum_x2y = 0, sum_xy2 = 0;

	for (const auto& point : points) {
		float x = point.x;
		float y = point.y;

		sum_x += x;
		sum_y += y;
		sum_x2 += x * x;
		sum_y2 += y * y;
		sum_x3 += x * x * x;
		sum_y3 += y * y * y;
		sum_xy += x * y;
		sum_x2y += x * x * y;
		sum_xy2 += x * y * y;
	}

	// 计算圆心坐标和半径的中间变量
	float C = n * sum_x2 - sum_x * sum_x;
	float D = n * sum_xy - sum_x * sum_y;
	float E = n * sum_x3 + n * sum_xy2 - (sum_x2 + sum_y2) * sum_x;
	float G = n * sum_y2 - sum_y * sum_y;
	float H = n * sum_x2y + n * sum_y3 - (sum_x2 + sum_y2) * sum_y;

	// 计算圆心坐标
	center->x = (H * D - E * G) / (C * G - D * D);
	center->y = (H * C - E * D) / (D * D - G * C);

	// 计算半径
	*radius = sqrt((center->x * center->x + center->y * center->y - (sum_x2 + sum_y2) / n));

	if (rmse)
	{
		double totalError = 0, maxError = 0;
		for (auto& p : points) {
			double dx = p.x - center->x, dy = p.y - center->y;
			double err = fabs(sqrt(dx * dx + dy * dy) - *radius);
			totalError += err * err;
			maxError = std::max(maxError, err);
		}
		*rmse = totalError / points.size();
	}

	return true;
}

VISION_EXPORT bool VisionApi::fitCircle_RANSAC(const std::vector<cv::Point2d>& points, float* center_x, float* center_y, double* radius, int maxIterCnt, double maxErrorThreshold)
{
	int cloudSize = points.size();
	int consensusCntThreshold = cloudSize * 0.95;

	if (!cloudSize || cloudSize <= 3)
	{
		return false;
	}

	//在点云链表的下标取值范围内产生随机数，目的是可以随机选择一个点云
	std::default_random_engine rng;
	std::uniform_int_distribution <int> uniform(0, cloudSize - 1);

	unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
	rng.seed(seed);  //seed函数可以接收任意整数作为实参

	std::vector<int> selectIndexs;            //随机选择的点云的索引
	std::vector<cv::Point2d> selectPoints;   //随机选择的点云对象

	double centerX = 0, centerY = 0, R = 0; //平面圆参数
	double modelMeanError = 9999;              //平均误差
	int bestConsensusCnt = 0;               //满足一致性的点的个数
	int iter = 0;                           //迭代次数

	//开始迭代计算
	while (iter < maxIterCnt)
	{
		selectIndexs.clear();
		selectPoints.clear();
		//在原始点云中随机取出3个点，因为至少需要3个点才能确定一个平面圆
		for (int c = 0; c < 3; ++c)
		{
			selectPoints.push_back(points.at(uniform(rng)));
		}

		float x1 = selectPoints[0].x;
		float y1 = selectPoints[0].y;

		float x2 = selectPoints.at(1).x;
		float y2 = selectPoints.at(1).y;

		float x3 = selectPoints.at(2).x;
		float y3 = selectPoints.at(2).y;

		//利用三个点坐标计算平面圆的参数
		double xa = (x1 + x2) / 2.0, ya = (y1 + y2) / 2.0;
		double xb = (x1 + x3) / 2.0, yb = (y1 + y3) / 2.0;

		double ka = (x1 - x2) / (y2 - y1);
		double kb = (x1 - x3) / (y3 - y1);

		centerX = (yb - ya + ka * xa - kb * xb) / (ka - kb);
		centerY = ka * centerX - ka * xa + ya;
		R = sqrt((centerX - xa) * (centerX - xa) + (centerY - ya) * (centerY - ya));

		double meanError = 0;
		int sums = 0;
		for (int j = 0; j < cloudSize; ++j)
		{
			cv::Point2d point = points.at(j);
			double distance = abs(R - sqrt((point.x - centerX) * (point.x - centerX) + (point.y - centerY) * (point.y - centerY)));
			if ((distance) < maxErrorThreshold)
			{
				sums++;
				meanError += distance;
			}

		}

		if (sums > cloudSize * 0.6 && meanError / sums < modelMeanError)
		{
			bestConsensusCnt = sums;  // 更新一致性索引集合元素个数
			modelMeanError = meanError / sums;
			*center_x = centerX;  //圆心X
			*center_y = centerY;  //圆心Y
			*radius = R;        //半径
		}

		if (sums > consensusCntThreshold)
		{
			break;
		}

		iter++;
	}

	return true;
}

ObjPos VisionApi::getObjPos(HImage src, HRegion roi, double thresh)
{
	ObjPos objP = { 0,0,0 };
	HImage roiImg = src.ReduceDomain(roi);
	HRegion reg = roiImg.Threshold(thresh, 255);
	HRegion openR = reg.OpeningCircle(25.5);
	HRegion closeR = openR.ClosingCircle(9.5);
	HRegion conn = closeR.Connection();
	HRegion objRegion = conn.SelectShapeStd("max_area", 70);
	if (objRegion.CountObj() == 0)
	{
		return objP;
	}

	double r, c, phi, len1, len2;
	objRegion.SmallestRectangle2(&r, &c, &phi, &len1, &len2);
	objP.x = c;
	objP.y = r;
	objP.angle = phi;
	objP.len1 = len1;
	objP.len2 = len2;

	// region的中心点：理论上语smallestrectangle2重合
	cv::Point2d pt;
	objRegion.AreaCenter(&pt.y, &pt.x);

	return objP;
}

std::vector<ObjPos> VisionApi::inspAllObjPos(HImage src)
{
	std::vector<ObjPos> allObjPos;
	ObjPos objP = { 0,0,0 };
	HRegion reg = src.Threshold(128, 255);
	HRegion openR = reg.OpeningCircle(3.5);
	HRegion closeR = openR.ClosingCircle(4.5);
	HRegion conn = closeR.Connection();
	//HRegion objRegion = conn.SelectShapeStd("max_area", 70);
	HRegion objRegions = conn.SelectShape("area", "and", 150 * 150, 250 * 250);
	if (objRegions.CountObj() == 0)
	{
		return allObjPos;
	}

	HTuple r, c, phi, len1, len2;
	objRegions.SmallestRectangle2(&r, &c, &phi, &len1, &len2);

	int n = r.Length();
	for (int i = 0; i < n; i++)
	{
		objP.y = r[i].D();
		objP.x = c[i].D();
		objP.angle = phi[i].D();
		objP.len1 = len1[i].D();
		objP.len2 = len2[i].D();

		allObjPos.push_back(objP);
	}

	return allObjPos;
}

HImage VisionApi::HImageFromMat(cv::Mat& src)
{
	HImage hImage;
	hImage.Clear();
	uchar* data = (uchar*)src.data;
	int width = src.cols;
	int height = src.rows;
	int bitDepth = src.channels() * 8;
	switch (bitDepth)
	{
	case 8:
		HalconCpp::GenImage1Extern(&hImage, "byte", width, height, (Hlong)data, NULL);
		break;
	case 16:
		HalconCpp::GenImage1Extern(&hImage, "int2", width, height, (Hlong)data, NULL);
		break;
	case 24:
		hImage.GenImageInterleaved(data, "bgr", width, height, -1, "byte", 0, 0, 0, 0, -1, 0);
		break;
	case 32:
		hImage.GenImageInterleaved(data, "bgrx", width, height, -1, "byte", 0, 0, 0, 0, -1, 0);
		break;
	default:
		std::cout << "不存在的位深度" << std::endl;
		break;
	}

	return hImage;
}


/**
 * @brief QImage2HImage 将 Qt QImage 转换为 Halcon 的 HImage
 * @param from 输入的 QImage
 * @param to 输出的 HImage ，from 和 to 不共享内存数据。 每次都会为 to 重新分配内存。
 * @return true 表示转换成功，false 表示转换失败。
 */
bool VisionApi::QImage2HImage(QImage& from, HalconCpp::HImage& to)
{
	if (from.isNull()) return false;

	int width = from.width(), height = from.height();
	QImage::Format format = from.format();

	if (format == QImage::Format_RGB32 ||
		format == QImage::Format_ARGB32 ||
		format == QImage::Format_ARGB32_Premultiplied)
	{
		if (from.bytesPerLine() == 4 * width)
		{
			to.GenImageInterleaved(from.bits(), "bgrx", width, height, 0, "byte", width, height, 0, 0, 8, 0);
		}
		else
		{
			to.GenImageInterleaved(from.bits(), "bgrx", width, height, 0, "byte", width, height, 0, 0, 8, 0);
			uchar* R, * G, * B;
			HString Type;
			Hlong Width, Height;
			to.GetImagePointer3(reinterpret_cast<void**>(&R),
				reinterpret_cast<void**>(&G),
				reinterpret_cast<void**>(&B), &Type, &Width, &Height);
			for (int row = 0; row < height; row++)
			{
				QRgb* line = reinterpret_cast<QRgb*>(from.scanLine(row));
				for (int col = 0; col < width; col++)
				{
					*R = qRed(line[col]);
					*G = qGreen(line[col]);
					*B = qBlue(line[col]);
					++R;
					++G;
					++B;
				}
			}
		}
		return true;
	}
	else if (format == QImage::Format_RGB888)
	{
		if (from.bytesPerLine() == 3 * width)
		{
			to.GenImageInterleaved(from.bits(), "rgb", width, height, 0, "byte", width, height, 0, 0, 8, 0);
		}
		else
		{
			to.GenImageInterleaved(from.bits(), "rgb", width, height, 0, "byte", width, height, 0, 0, 8, 0);
			uchar* R, * G, * B;
			HString Type;
			Hlong Width, Height;
			to.GetImagePointer3(reinterpret_cast<void**>(&R),
				reinterpret_cast<void**>(&G),
				reinterpret_cast<void**>(&B), &Type, &Width, &Height);
			for (int row = 0; row < height; row++)
			{
				unsigned char* line = reinterpret_cast<unsigned char*>(from.scanLine(row));
				for (int col = 0; col < width; col++)
				{
					*R++ = *line++;
					*G++ = *line++;
					*B++ = *line++;
				}
			}
		}
		return true;
	}
	else if (format == QImage::Format_Grayscale8 || format == QImage::Format_Indexed8)
	{
		if (from.bytesPerLine() == width)
		{
			to.GenImage1("byte", width, height, from.bits());
		}
		else// 这时说明每行数据之间有填充字节。因此需要重新写数据
		{
			to.GenImageConst("byte", width, height);
			Hlong W, H; HString Type;
			unsigned char* pTo = reinterpret_cast<unsigned char*>(to.GetImagePointer1(&Type, &W, &H));
			for (int row = 1; row < H; row++)
			{
				const unsigned char* pSrc = from.constScanLine(row);
				unsigned char* pDist = pTo + row * W;
				memcpy(pDist, pSrc, static_cast<size_t>(W));
			}
		}
		return true;
	}
	return false;
}

/**
 * @brief HImage2QImage 将 Halcon 的 HImage 转换为 Qt 的 QImage
 * @param from HImage ，暂时只支持 8bits 灰度图像和 8bits 的 3 通道彩色图像
 * @param to QImage ，这里 from 和 to 不共享内存。如果 to 的内存大小合适，那么就不用重新分配内存。所以可以加快速度。
 * @return  true 表示转换成功，false 表示转换失败
 */
bool VisionApi::HImage2QImage(HalconCpp::HImage& from, QImage& to)
{
	Hlong width;
	Hlong height;
	from.GetImageSize(&width, &height);

	HTuple channels = from.CountChannels();
	HTuple type = from.GetImageType();

	if (strcmp(type[0].S(), "byte")) // 如果不是 byte 类型，则失败
	{
		return false;
	}

	QImage::Format format;
	switch (channels[0].I())
	{
	case 1:
		format = QImage::Format_Grayscale8;
		break;
	case 3:
		format = QImage::Format_RGB888;
		break;
	default:
		return false;
	}

	if (to.width() != width || to.height() != height || to.format() != format)
	{
		to = QImage(static_cast<int>(width),
			static_cast<int>(height),
			format);
	}
	HString Type;
	if (channels[0].I() == 1)
	{
		unsigned char* pSrc = reinterpret_cast<unsigned char*>(from.GetImagePointer1(&Type, &width, &height));
		if (to.bytesPerLine() == width)
		{
			memcpy(to.bits(), pSrc, static_cast<size_t>(width) * static_cast<size_t>(height));
		}
		else
		{
			for (int row = 1; row < height; row++)
			{
				unsigned char* pDistLine = to.scanLine(row);
				const unsigned char* pSrcLine = pSrc + row * width;
				memcpy(pDistLine, pSrcLine, static_cast<size_t>(width));
			}
		}

		return true;
	}
	else if (channels[0].I() == 3)
	{
		uchar* R, * G, * B;
		from.GetImagePointer3(reinterpret_cast<void**>(&R),
			reinterpret_cast<void**>(&G),
			reinterpret_cast<void**>(&B), &Type, &width, &height);

		for (int row = 0; row < height; row++)
		{
			unsigned char* line = reinterpret_cast<unsigned char*>(to.scanLine(row));
			for (int col = 0; col < width; col++)
			{
				*line++ = *R++;
				*line++ = *G++;
				*line++ = *B++;
			}
		}
		return true;
	}

	return false;
}

QImage VisionApi::mat2QImage(cv::Mat& mat)
{
	switch (mat.type()) {
	case CV_8UC1: // 灰度图
		return QImage(mat.data, mat.cols, mat.rows,
			mat.step, QImage::Format_Grayscale8);
	case CV_8UC3: // BGR彩色图
		return QImage(mat.data, mat.cols, mat.rows,
			mat.step, QImage::Format_RGB888).rgbSwapped();
	case CV_8UC4: // 带Alpha通道
		return QImage(mat.data, mat.cols, mat.rows,
			mat.step, QImage::Format_ARGB32);
	default:
		return QImage();
	}
}

cv::Mat VisionApi::qImage2Mat(QImage& image)
{
	cv::Mat cvMat;
	//qDebug() << image.format();
	switch (image.format())
	{
	case QImage::Format_ARGB32:
	case QImage::Format_RGB32:
		cvMat = cv::Mat(image.height(), image.width(), CV_8UC4, (void*)image.constBits(), image.bytesPerLine());
		cv::cvtColor(cvMat, cvMat, cv::COLOR_BGRA2BGR);
		break;
	case QImage::Format_ARGB32_Premultiplied:
		cvMat = cv::Mat(image.height(), image.width(), CV_8UC4, (void*)image.constBits(), image.bytesPerLine());
		break;
	case QImage::Format_RGB888:
		cvMat = cv::Mat(image.height(), image.width(), CV_8UC3, (void*)image.constBits(), image.bytesPerLine());
		cv::cvtColor(cvMat, cvMat, cv::COLOR_BGR2RGB);
		break;
	case QImage::Format_Indexed8:
		cvMat = cv::Mat(image.height(), image.width(), CV_8UC1, (void*)image.constBits(), image.bytesPerLine());
		break;
	case QImage::Format_Grayscale8:
		cvMat = cv::Mat(image.height(), image.width(), CV_8UC1, (void*)image.constBits(), image.bytesPerLine());
		break;
	}
	return cvMat;
}

double VisionApi::rad2Deg(double rad)
{
	HTuple deg;
	TupleDeg(rad, &deg);
	return deg.D();
}

double VisionApi::deg2Rad(double deg)
{
	HTuple rad;
	TupleRad(deg, &rad);
	return rad.D();
}

VISION_EXPORT void VisionApi::rotatePoint(double x, double y, double rotAngle, double* outX, double* outY)
{
	*outX = x * cos(rotAngle) - y * sin(rotAngle);
	*outY = x * sin(rotAngle) + y * cos(rotAngle);
	return;
}

VISION_EXPORT void VisionApi::rotatePoint(double x, double y, double refX, double refY, double rotAngle, double* outX, double* outY)
{
	*outX = (x - refX) * cos(rotAngle) - (y - refY) * sin(rotAngle) + refX;
	*outY = (x - refX) * sin(rotAngle) + (y - refY) * cos(rotAngle) + refY;
	return;
}

VISION_EXPORT void VisionApi::rotatePoint(const cv::Point2d& p, double rotAngle, cv::Point2d* outPoint)
{
	rotatePoint(p.x, p.y, rotAngle, &outPoint->x, &outPoint->y);
	return;
}

VISION_EXPORT cv::Point2d VisionApi::rotatePoint(const cv::Point2d& p, double rotAngle)
{
	cv::Point2d pt;
	rotatePoint(p.x, p.y, rotAngle, &pt.x, &pt.y);
	return pt;
}

VISION_EXPORT void VisionApi::rotatePoint(const cv::Point2d& p, double rotAngle, const cv::Point2d& refPoint, cv::Point2d* outPoint)
{
	return rotatePoint(p.x, p.y, refPoint.x, refPoint.y, rotAngle, &outPoint->x, &outPoint->y);
}

VISION_EXPORT cv::Point2d VisionApi::rotatePoint(const cv::Point2d& p, double rotAngle, const cv::Point2d& refPoint)
{
	cv::Point2d pt;
	rotatePoint(p.x, p.y, refPoint.x, refPoint.y, rotAngle, &pt.x, &pt.y);
	return pt;
}

VISION_EXPORT cv::Point2d VisionApi::arrowEndPoint(const cv::Point2d& startPt, double len, double angle)
{
	return rotatePoint(cv::Point2d(len, 0), angle)+startPt;
}

void VisionApi::drawCross(cv::Mat& src, cv::Point pt, cv::Scalar color, cv::Size sz)
{
	cv::line(src, cv::Point(pt.x - sz.width, pt.y), cv::Point(pt.x + sz.width, pt.y), color);
	cv::line(src, cv::Point(pt.x - sz.width, pt.y), cv::Point(pt.x + sz.width, pt.y), color);
	cv::line(src, cv::Point(pt.x, pt.y - sz.height), cv::Point(pt.x, pt.y + sz.height), color);
}

void VisionApi::textOut(cv::Mat& src, cv::Point pt, cv::Scalar color)
{
	//cv::putText(image,
	//	text,
	//	cv::Point(image.cols - 200, 30), // 确保位置不超出图像范围
	//	cv::FONT_HERSHEY_SIMPLEX,
	//	0.7,
	//	cv::Scalar(0, 0, 255), // 红色
	//	2);
}

VISION_EXPORT void VisionApi::genPolygonFromRect2(cv::Point2d* points, const RotRect& rect2)
{
	cv::Point2d newRect[4];
	newRect[0] = cv::Point2d(-rect2.len1, -rect2.len2);
	newRect[1] = cv::Point2d(rect2.len1, -rect2.len2);
	newRect[2] = cv::Point2d(rect2.len1, rect2.len2);
	newRect[3] = cv::Point2d(-rect2.len1, rect2.len2);

	const cv::Point2d center(rect2.x,rect2.y);
	double a = rect2.angle;

	points[1] = rotatePoint(newRect[1], -rect2.angle) + center;
	points[2] = rotatePoint(newRect[2], -rect2.angle) + center;
	points[0] = 2 * center - points[2];
	points[3] = 2 * center - points[1];
}

VISION_EXPORT void VisionApi::drawRotRect(cv::Mat& src, RotRect r, cv::Scalar color)
{
	cv::Point2d vertices[4];
	genPolygonFromRect2(vertices, r);
	for (int i = 0; i < 4; i++) {
		cv::line(src, vertices[i], vertices[(i + 1) % 4], color, 2);
	}

	// 绘制矩形方向：角度前加-，halcon计算的正角度是逆时针，图像中正角度为顺时针
	cv::arrowedLine(src, cv::Point(r.x, r.y), arrowEndPoint(cv::Point(r.x, r.y), r.len1, -r.angle), color);

	return ;
}

void VisionApi::drawHalconXld(cv::Mat& src, HXLDCont& xld, cv::Scalar color)
{
	int objCount = xld.CountObj();
	for (int i = 0; i < objCount; i++)
	{
		auto obj = xld.SelectObj(i + 1);

		cvContour c;
		HTuple rows, cols;
		obj.GetContourXld(&rows, &cols);
		int len = rows.Length();
		for (int j = 0; j < len; j++)
		{
			c.push_back(cv::Point(cols[j].D(), rows[j].D()));
		}

		cv::polylines(src, c, false, color, 2, cv::LINE_AA, 0);
	}
}

void VisionApi::drawOpencvContour(cv::Mat& src, std::vector<polygonPoints>& allContours, cv::Scalar color)
{
	for (auto& ply : allContours)
	{
		std::vector<cv::Point> points;
		for (auto& p : ply)
		{
			points.push_back(p);
		}
		cv::polylines(src, points, false, color, 1, cv::LINE_AA, 0);
	}
}

//VISION_EXPORT void VisionApi::drawInferResults(cv::Mat& src, DetectResultList& inferResults)
//{
//	for (auto &detection : inferResults)
//	{
//		cv::Rect box = detection.box;
//		cv::Scalar color = detection.color;
//
//		// Detection box
//		cv::rectangle(src, box, color, 2);
//
//		// Detection box text
//		std::string classString = detection.className + ' ' + std::to_string(detection.confidence).substr(0, 4);
//		cv::Size textSize = cv::getTextSize(classString, cv::FONT_HERSHEY_DUPLEX, 1, 2, 0);
//		cv::Rect textBox(box.x, box.y - 40, textSize.width + 10, textSize.height + 20);
//
//		cv::rectangle(src, textBox, color, cv::FILLED);
//		cv::putText(src, classString, cv::Point(box.x + 5, box.y - 10), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(0, 0, 0), 2, cv::LINE_AA, false);
//
//		//qInfo() << classString;
//	}
//
//	return;
//}
